#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <unistd.h>
#include <strings.h>
#include <stdlib.h>
#include <pthread.h>

#define BAUDRATE B57600  /* the baud rate */
#define ROBOTPORT "/dev/rfcomm0" /* port for bluetooth connection */
#define _POSIX_SOURCE 1 /* POSIX compliant source */
#define true 1 
#define false 0 

#define HEADER_PROMPT "\n\n\n\nPress f to move the robot forward, b to go backwards, s to stop the robot.\n"
#define EXIT_ERROR "Communication with Robot failed.  Exiting program!\n"
#define IN_LENGTH 8 /* Length of the buffers to be used for input and output. */
#define OUT_LENGTH 8 /* Length of the buffers to be used for input and output. */
#define MAIN_BUFFER_LENGTH 100 /* Length of the buffers to be used for input and output. */

unsigned char mainBuffer[MAIN_BUFFER_LENGTH];
int bufferStart = 0, bufferEnd = 0;
int running;
int robot;
int showData = 1;
static pthread_t sensorThread;
static pthread_t keyThread;
static pthread_t moveThread;
static pthread_mutex_t buffer_mutex;
static pthread_mutex_t buffer_empty_mutex;
static pthread_mutex_t buffer_full_mutex;

struct termios config;


void toggleData()
{
	if (showData == 0)
		showData = 1;
	else
		showData = 0;
}



int start(int robot)
{
	config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
	config.c_oflag = 0;
	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
	config.c_cflag &= ~(CSIZE | PARENB);
	config.c_cflag |= CS8;
	config.c_cc[VMIN]  = 1;
	config.c_cc[VTIME] = 0;
	if (cfsetispeed(&config, BAUDRATE) < 0 || cfsetospeed(&config, BAUDRATE) < 0) { 
	/*return -2 if could not set baud rate*/
		return -2;
	}
	if(tcsetattr(robot, TCSAFLUSH, &config) < 0) { /*return -3 if could not set configurations*/
		return -3;
	}
	printf("Starting the open interface...\n");
	unsigned char out[OUT_LENGTH];
	out[0] = 128;
	out[1] = 131;
	out[2] = 139;
	out[3] = 8;
	out[4] = 0;
	out[5] = 255;
	write(robot, out, 6); 
	return 1;
}


void header()
{
	printf(HEADER_PROMPT);
}


void keyError(char key){
	printf("%c is an invalid key!\n", key);
   }

void readSensorData()
{
}

void forward() 
{
	unsigned char out[] = {145, 0, 100, 0, 100};
	write(robot, out, 5); 
}

void backward() 
{
	unsigned char out[] = {145, 255, 156, 255, 156};
	write(robot, out, 5); 
}

void left() 
{
	unsigned char out[] = {145, 0, 100, 255, 156};
	write(robot, out, 5); 
}

void right() 
{
	unsigned char out[] = {145, 255, 156, 0, 100};
	write(robot, out, 5); 
}

void stop() 
{
	unsigned char out[] = {145, 0, 0, 0, 0};
	write(robot, out, 5); 
}



/*
void forward()
{
	unsigned char out[OUT_LENGTH];
	out[0] = 137;
	out[1] = 100;
	out[2] = 0;
	out[3] = 28;
	out[4] = 0;
	write(robot, out, 5); 
}

void stop()
{
	unsigned char out[OUT_LENGTH];
	out[0] = 137;
	out[1] = 100;
	out[2] = 0;
	out[3] = 0;
	out[4] = 0;
	write(robot, out, 5); 
}
*/


void timestampSensorPrint(int in, int i)
{
    time_t ltime; /* calendar time */
	ltime=time(NULL); /* get current cal time */
	if(showData)
		printf("%s - %s signal: %d\n", asctime(localtime(&ltime)), (i ? "Right" : "Left"), in );
}


void *sensorStream(void *params)
{
	unsigned char out[OUT_LENGTH], in[OUT_LENGTH];
	unsigned char temp;
	int right, left;
	out[0] = 142;
	out[1] = 7;
	write(robot, out, 2); 
	while(true)
	{
		read(robot, in, IN_LENGTH);  /* should only return 1 bit.*/
		temp = in[0];
		right = (temp % 2) != 0; /*checks the first bit*/
		temp = (temp / 2); /* shifts right */
		left = (temp % 2) != 0; /* checks the second bit*/
		timestampSensorPrint(right, 1);
		timestampSensorPrint(left, 0);
		usleep(1000000/20);
		out[0] = 142;
		out[1] = 7;
		write(robot, out, 2); 
	}
}

char getBufferItem()
{
	while (bufferStart == bufferEnd )
	{
	}
	//printf("item removed from buffer\n");
	/* CRITICAL SECTION */
	pthread_mutex_lock(&buffer_mutex);
	char temp = mainBuffer[bufferStart];
	bufferStart++;
	bufferStart = bufferStart % MAIN_BUFFER_LENGTH;
	pthread_mutex_unlock(&buffer_mutex);
	//pthread_mutex_unlock(&buffer_full_mutex);
	return temp;
}

void addBufferItem(char input)
{
	//printf("item added to buffe\n");
	while(((bufferEnd + 1) % MAIN_BUFFER_LENGTH) == bufferStart)
	{
		//pthread_mutex_lock(&buffer_full_mutex);
		//printf("Buffer is full! Please wait!");
	}
	/* CRITICAL SECTION */
	pthread_mutex_lock(&buffer_mutex);
	mainBuffer[bufferEnd] = input;
	bufferEnd++;
	bufferEnd = bufferEnd % MAIN_BUFFER_LENGTH;
	pthread_mutex_unlock(&buffer_mutex);
	//pthread_mutex_unlock(&buffer_empty_mutex);
}

int isValidKey(char key)
{
	switch (key)
	{
		case 'f':
			return 1;
		case 'b':
			return 1;
		case 's':
			return 1;
		//case 'e':
			//return 1;
		case 'p':
			return 1;
		case 'l':
			return 1;
		case 'r':
			return 1;
		case '\n':
			return 0;
		default:
			keyError(key);	
			return 0;
	}
	return 0;
}

void *keyThreadFunction()
{
	char key;
	while (running)
	{
		key = (char) getc(stdin);
		if (isValidKey(key) != 0)
			addBufferItem(key);
	}
}

void *moveThreadFunction()
{
	char key;
	while (running)
	{
		/*get item from buffer*/
		key = (char)getBufferItem();
		/*Call appropriate command*/
		switch (key)
		{
			case 'f':
				forward();	
				break;
			case 'b':
				backward();	
				break;
			case 's':
				stop();	
				break;
			case 'l':
				left();	
				break;
			case 'r':
				right();	
				break;
			case 'p':
				toggleData();	
				break;
			case 'e':
				break;
		}
	}
}



int main()
{
	pthread_mutex_init(&buffer_mutex, NULL);
	pthread_mutex_init(&buffer_full_mutex, NULL);
	pthread_mutex_init(&buffer_empty_mutex, NULL);
	running = 1;
	int started;
	header();
	robot = open(ROBOTPORT, O_RDWR | O_NOCTTY | O_NDELAY);
	if (robot < 0)
	{
		printf(EXIT_ERROR);
		return -1;
	}
	started = start(robot);
	if (started < 0)
	{
		return -2;
	}
	int a = pthread_create(&sensorThread, NULL, sensorStream, NULL);

	/*Start the Movement and key threads*/
	int b = pthread_create(&keyThread, NULL, keyThreadFunction, NULL);
	int c = pthread_create(&moveThread, NULL, moveThreadFunction, NULL);

	/*Get Sensor data*/

	while (true)
	{
		sleep(1);
	}
	return 0;
}
